#include "Registration.hpp"
#include "SubdivisionAlgorithm.hpp"
#include "PolyMesh.hpp"
#include "linearsystem.h"
#include <stdio.h>
#include "DebugHelper.hpp"

#include "boost/multi_array.hpp"
#include "boost/numeric/ublas/matrix.hpp"
#include "boost/numeric/ublas/matrix_proxy.hpp"
#include "boost/numeric/ublas/matrix_sparse.hpp"
#include "MeshAlgorithm.hpp"

namespace ublas = boost::numeric::ublas;

namespace
{
   const int cSUB_LEVEL = 3;
   const int tettris[4][3]={{0,1,2},{0,2,3},{0,3,1},{1,3,2}};
   const int tettip[4] =    {3,      1,      2,      0};
   const int octtris[8][3]={{0,1,2},{3,4,5},{0,4,3},{0,2,4},{2,5,4},{2,1,5},{1,3,5},{1,0,3}};

   static float getVolume( float v1[3], float v2[3], float v3[3], float v4[3] )
   {
      // return 1 ;

      // Form area-based normal
      float a[3], b[3], c[3], n[3] ;
      for ( int j = 0 ; j < 3 ; j ++ )
      {
         a[j] = v2[ j ] - v1[ j ] ; 
         b[j] = v3[ j ] - v2[ j ] ; 
         c[j] = v3[ j ] - v4[ j ] ;
      }

      n[0] = a[1] * b[2] - a[2] * b[1] ;
      n[1] = a[2] * b[0] - a[0] * b[2] ;
      n[2] = a[0] * b[1] - a[1] * b[0] ;

      return ( c[0] * n[0] + c[1] * n[1] + c[2] * n[2] ) / 6 ;
   }


   struct alignCode
   {
      // subdivision mesh vertices
      int numVerts;
      float* vertlist;

      // control mesh vertices
      int numOriVerts;
      float* orivertlist;

      // control mesh parameters ?:
      int numCrTris;
      int* crtrilist;

      int* facemasks;
      int numCrVerts;
      int* crvertlist;
      int numCrEdges;
      int* credgelist;
      int numTets;
      int* tetlist;
      int numOcts;
      int* octlist;
      int* tetmasks ;
      int* octmasks ;
      int sublevel;

      // Landmark vertices
      int numLMVerts;
      float* lmvertlist;

      alignCode()
      {
         tetmasks = 0;
         octmasks = 0;
         numOcts = 0;
         numTets = 0;
         numCrEdges = 0;
         numCrVerts = 0;
         numCrTris = 0;
         numOriVerts = 0;
         numLMVerts = 0;
      }
   /************************************************************************/
   /* Fit with least-square
   /************************************************************************/
   void alignLS(float fitWt, float energyWt, float crpWt)
   {
      //corresponding pts list

      //	sortVector( selectLmkVerList );	
      //	sortVector( selectSubVerList );
      int i,j,k ;
      int maxIterations = 100 ;
      //    float energyWt = 1 ;	
      //	float fitWt = 0; //for debug, first set it to be 0.
      //  float fitWt = 40 ;
      /*part 3: corresponding pts*/
      //	float crpWt = 100;

      printf("fitWt: %f\nenergyWt:%f\nCrpWt:%f\n", fitWt, energyWt, crpWt);
      int fitSubLevel = 3 ;
      int energySubLevel = 2 ;

      // Find interior vertices
      int* vertflag = new int[ this->numOriVerts ] ;
      //int centind = -1 ;
      for ( i = 0 ; i < numOriVerts ; i ++ )
      {
         vertflag[ i ] = 0 ;
      }
      for ( i = 0 ; i < numCrTris ; i ++ )
      {
         for ( j = 0 ; j < 3 ; j ++ )
         {
            vertflag[ crtrilist[ 3 * i + j ] ] = 1 ;
         }
      }
      /*
      for ( i = 0 ; i < numOriVerts ; i ++ )
      {
      if ( vertflag[ i ] == 0 )
      {
      centind = i ;
      break ;
      }
      }
      printf("Center index: %d\n", centind) ;
      */

      // allocate ata, atb, x and temporary vars
      printf("Allocating...\n") ;
      int numvar = numOriVerts ;
      float** ata = new float * [ numvar ] ;
      float** atb = new float * [ 3 ] ;
      float* x = new float [ numvar ] ;
      for ( i = 0 ; i < 3 ; i ++ )
      {
         atb[i] = new float[ numvar ] ;
         for ( j = 0 ; j < numvar ; j ++ )
         {
            atb[i][j] = 0 ;
         }
      }
      for ( i = 0 ; i < numvar ; i ++ )
      {
         ata[i] = new float[ numvar ] ;
         for ( j = 0 ; j < numvar ; j ++ )
         {
            ata[i][j] = 0 ;
         }
      }

      // Subdivide mask
      printf("Subdividing...\n") ;
      float* maskl = new float[ numOriVerts * numOriVerts ] ;
      for ( i = 0 ; i < numOriVerts ; i ++ )
      {
         for ( j = 0 ; j < numOriVerts ; j ++ )
         {
            maskl[ i * numOriVerts + j ] = 0 ;
         }
         maskl[ i * numOriVerts + i ] = 1 ;
      }

      int nSubVerts, nCrVerts, nCrEdges, nCrTris, nTets, nOcts ;
      int *crvertl, *credgel, *crtril, *tetl, *octl ;

      nSubVerts = numOriVerts ;
      nCrVerts = numCrVerts ;
      nCrEdges = numCrEdges ;
      nCrTris = numCrTris ;
      nTets = numTets ;
      nOcts = numOcts ;

      crvertl = new int[ numCrVerts ] ;
      credgel = new int[ numCrEdges * 2 ] ;
      crtril = new int[ numCrTris * 3 ] ;
      tetl = new int[ numTets * 4 ] ;
      octl = new int[ numOcts * 6 ] ;
      if ( tetmasks != NULL )
      {
         delete tetmasks ;
         delete octmasks ;
      }
      tetmasks = new int[ numTets ] ;
      octmasks = new int[ numOcts ] ;
      for ( i = 0 ; i < numCrVerts ; i ++ )
      {
         crvertl[i] = crvertlist[i] ;
      }
      for ( i = 0 ; i < numCrEdges * 2 ; i ++ )
      {
         credgel[i] = credgelist[i] ;
      }
      for ( i = 0 ; i < numCrTris * 3 ; i ++ )
      {
         crtril[i] = crtrilist[i] ;
      }
      for ( i = 0 ; i < numTets * 4 ; i ++ )
      {
         tetl[i] = tetlist[i] ;
      }
      for ( i = 0 ; i < numOcts * 6 ; i ++ )
      {
         octl[i] = octlist[i] ;
      }
      for ( i = 0 ; i < numTets ; i ++ )
      {
         tetmasks[i] = 0 ;
      }
      for ( i = 0 ; i < numOcts ; i ++ )
      {
         octmasks[i] = 0 ;
      }
      //tetmasks[ currentTet ] = 1 ;
      DebugHelper::CheckMemory();
      for ( i = 0 ; i < sublevel ; i ++ )
      {
         SubdivisionAlgorithm::subdivide( nSubVerts, maskl, nCrVerts, crvertl, nCrEdges, credgel, nCrTris,
            crtril, nTets, tetl, nOcts, octl, tetmasks, octmasks, facemasks, numOriVerts ) ;
      }
      DebugHelper::CheckMemory();
      delete crvertl ;
      delete credgel ;
      delete crtril ;
      delete tetl ;
      delete octl ;

      if ( nSubVerts != numVerts )
      {
         printf("Wrong! nSubVerts: %d, numVerts: %d\n", nSubVerts, numVerts ) ;
      }

      // Prepare to add terms
      int numIntFaces = (4 * numTets - numCrTris) / 2 ;
      // int numrows = numLMVerts + numIntFaces ;
      /****************/
      // DEH COMMENTED OUT FOLLOWING LINE:
      //int numCrp = selectLmkVerList.size() < selectSubVerList.size()? selectLmkVerList.size():selectSubVerList.size();
      int numCrp = 0;
      int numrows = numLMVerts + numCrp + numIntFaces;
      /****************/

      // Add fit terms
      printf("Adding fit terms...\n") ;
      float** a = new float * [ numrows ] ;
      float** b = new float * [3] ;
      for ( i = 0 ; i < 3 ; i ++ )
      {
         b[i] = new float [ numrows ] ;
      }
      DebugHelper::CheckMemory();
      for ( i = 0 ; i < numLMVerts ; i ++ )
      {
         // for each landmark, find closest mesh vertex
         float* lm = &(lmvertlist[ 3 * i ]) ;
         int ind = -1 ;
         float mindis = 1000000 ;
         for ( j = 0 ; j < numVerts ; j ++ )
         {
            // Only consider surface vertices
            int k = 0 ;
            for ( k = 0 ; k < numOriVerts ; k ++ )
            {
               if ( vertflag[k] == 0 && maskl[ numOriVerts * j + k ] != 0 )
               {
                  break ;
               }
            }
            if ( k < numOriVerts )
            {
               continue ;
            }

            float* v = &(vertlist[ 3 * j ]) ;
            float d[3] ;
            if ( ( d[0] = fabs( lm[0] - v[0] ) ) < mindis 
               && ( d[1] = fabs( lm[1] - v[1] ) ) < mindis
               && ( d[2] = fabs( lm[2] - v[2] ) ) < mindis )
            {
               float dis = sqrt( d[0] * d[0] + d[1] * d[1] + d[2] * d[2] ) ;
               if ( dis < mindis )
               {
                  mindis = dis ;
                  ind = j ;
               }
            }
         }

         // Get A
         a[ i ] = new float[ numvar ] ;
         for ( j = 0 ; j < numvar ; j ++ )
         {
            a[i][j] = maskl[ numOriVerts * ind + j ] * fitWt ;
         }

         // Get b for each x,y,z component
         for ( j = 0 ; j < 3 ; j ++ )
         {
            b[j][i] = lm[ j ] * fitWt ;
         }
      }

      // Add energy terms
      printf("Adding energy terms...\n") ;

      // Find all interior faces
      int tettip[4]={3,1,2,0};
      int* intfaces = new int[ (numIntFaces + numCrTris) * 3 ] ;
      int* pairs = new int[ (numIntFaces + numCrTris) * 2 ] ;
      int ct = 0 ;
      DebugHelper::CheckMemory();
      for ( i = 0 ; i < this->numTets ; i ++ )
      {
         for ( j = 0 ; j < 4 ; j ++ )
         {
            int tri[3] = { tetlist[ i * 4 + tettris[j][0] ], tetlist[ i * 4 + tettris[j][1]], tetlist[ i * 4 + tettris[j][2] ] } ; 
            int sum = tri[0] + tri[1] + tri[2] ;
            for ( k = 0 ; k < ct ; k ++ )
            {
               // Check to see if the triangle was found before
               if ( (intfaces[ k * 3 ] + intfaces[ k * 3 + 1 ] + intfaces[ k * 3 + 2 ]) != sum )
               {
                  continue ;
               }
               int isSame = 0 ;
               for ( int kk = 0 ; kk < 3 ; kk ++ )
               {
                  if ( intfaces[ k * 3 + kk] == tri[ 0 ] || 
                     intfaces[ k * 3 + kk] == tri[ 1 ] || 
                     intfaces[ k * 3 + kk] == tri[ 2 ] )
                  {
                     isSame ++ ;
                  }
               }
               if ( isSame == 3 )
               {
                  pairs[ 2 * k + 1 ] = tetlist[ i * 4 + tettip[ j ] ] ;
                  break ;                                
               }
            }
            if ( k == ct )
            {
               intfaces[ k * 3 ] = tri[ 0 ] ;
               intfaces[ k * 3 + 1 ] = tri[ 1 ] ;
               intfaces[ k * 3 + 2 ] = tri[ 2 ] ;
               pairs[ k * 2 ] = tetlist[ i * 4 + tettip[ j ] ] ;
               pairs[ k * 2 + 1 ] = -1 ;
               ct ++ ;
            }
         }
      }
      printf("Should have %d distinct faces, now have %d.\n", (numIntFaces + numCrTris),ct) ;

      // Add one term for each face
      /*
      for ( i = 0 ; i < numrows ; i ++ )
      {
      delete a[i] ;
      }
      for ( i = 0 ; i < 3 ; i ++ )
      {
      delete b[i] ;
      }
      delete a ;
      delete b ;
      numrows = numIntFaces
      a = new float * [ numrows ] ;
      b = new float * [3] ;
      for ( i = 0 ; i < 3 ; i ++ )
      {
      b[i] = new float [ numrows ] ;
      }
      */
      DebugHelper::CheckMemory();
      int ind = numLMVerts ;
      for ( i = 0 ; i < (numIntFaces + numCrTris) ; i ++ )
      {
         // Skip if it's an outside face
         if ( pairs[ i * 2 + 1 ] < 0 )
         {
            continue ;
         }

         // Get relevant vertices
         float p1[3], p2[3], m1[3], m2[3], m3[3] ;
         int ip1 = pairs[ i * 2 ] ;
         int ip2 = pairs[ i * 2 + 1 ] ;
         int im1 = intfaces[ i * 3 ] ;
         int im2 = intfaces[ i * 3 + 1 ] ;
         int im3 = intfaces[ i * 3 + 2 ] ;
         for ( j = 0 ; j < 3 ; j ++ )
         {
            p1[j] = orivertlist[ ip1 * 3 + j ] ;
            p2[j] = orivertlist[ ip2 * 3 + j ] ;
            m1[j] = orivertlist[ im1 * 3 + j ] ;
            m2[j] = orivertlist[ im2 * 3 + j ] ;
            m3[j] = orivertlist[ im3 * 3 + j ] ;
         }

         // Get A
         a[ ind ] = new float[ numvar ] ;
         for ( j = 0 ; j < numvar ; j ++ )
         {
            a[ind][j] = 0 ;
         }

         /* Ensure correct orientation
         float v1[3], v2[3], n[3] ;
         for ( j = 0 ; j < 3 ; j ++ )
         {
         v1[j] = m2[j] - m1[j] ; 
         v2[j] = m3[j] - m2[j] ; 
         }
         n[0] = v1[1] * v2[2] - v1[2] * v2[1] ;
         n[1] = v1[2] * v2[0] - v1[0] * v2[2] ;
         n[2] = v1[0] * v2[1] - v1[1] * v2[0] ;
         float proj = n[0] * ( p1[0] - m1[0] ) + n[1] * ( p1[1] - m1[1] ) + n[2] * ( p1[2]
         - m1[2] ) ;
         if ( proj < 0 )
         {
         for ( j = 0 ; j < 3 ; j ++ )
         {
         int ftemp = p1[j] ;
         p1[j] = p2[j] ;
         p2[j] = ftemp ;
         }
         int temp = ip1 ;
         ip1 = ip2 ;
         ip2 = temp ;
         }
         */

         // Get different volumes
         float vol1 = getVolume( m3,m2,m1,p2 ) ;
         float vol2 = getVolume( m1,m2,m3,p1 ) ;
         float vol3 = getVolume( p1,m3,m2,p2 ) ;
         float vol4 = getVolume( p1,m1,m3,p2 ) ;
         float vol5 = getVolume( p1,m2,m1,p2 ) ;
         float normvol = vol1 + vol2 ;
         normvol *= normvol ;
         a[ind][ ip1 ] = vol1 / normvol * energyWt ;
         a[ind][ ip2 ] = vol2 / normvol * energyWt ;
         a[ind][ im1 ] = -vol3 / normvol * energyWt ;
         a[ind][ im2 ] = -vol4 / normvol * energyWt ;
         a[ind][ im3 ] = -vol5 / normvol * energyWt ;

         /* Debug
         for ( j = 0 ; j < 3 ; j ++ )
         {
         float sum = a[ind][ip1] * orivertlist[3*ip1+j] +
         a[ind][ip2] * orivertlist[3*ip2+j] +
         a[ind][im1] * orivertlist[3*im1+j] +
         a[ind][im2] * orivertlist[3*im2+j] +
         a[ind][im3] * orivertlist[3*im3+j] ;
         if ( sum != 0 )
         {
         printf("Wong! sum: %f\n", sum) ;
         }
         }
         */

         // Get b for each x,y,z component
         for ( j = 0 ; j < 3 ; j ++ )
         {
            b[j][ind] = 0;
         }
         DebugHelper::CheckMemory();
         ind ++ ;
      }

      //Add corresponding pts fit terms
      printf("Add corresponding pts fit terms...\n");
      // Get A
      
      ////// DEH numCRP = 0
      //////for( i = 0; i < numCrp; i++)	//corresponding pts
      //////{
      //////   a[ ind ] = new float[ numvar ] ;
      //////   int curSelectSubVer = selectSubVerList[i];
      //////   for ( j = 0 ; j < numvar ; j ++ )
      //////   {
      //////      a[ind][j] = maskl[ numOriVerts * curSelectSubVer + j ] * crpWt ;
      //////   }

      //////   // Get b for each x,y,z component
      //////   float* lm = &lmvertlist[selectLmkVerList[i]*3];
      //////   for ( j = 0 ; j < 3 ; j ++ )
      //////   {
      //////      b[j][ind] = lm[ j ] * crpWt ;
      //////   }
      //////   ind ++;
      //////}

      printf("Should have %d rows, now has %d.\n", numrows, ind) ;

      // Add to ata, atb
      printf("Adding to matrix...\n") ;
      DebugHelper::CheckMemory();
      LinearSystemSolver::matrixMultiply( numrows, numvar, a, numrows, numvar, a, ata,1, 0 ) ;
      for ( j = 0 ; j < 3 ; j ++ )
      {
         LinearSystemSolver::matrixMultiply( numrows, numvar, a, numrows, b[j], atb[j], 1) ;
      }
      DebugHelper::CheckMemory();

      // Solving
      printf("Solving...\n") ;
      for ( j = 0 ; j < 3 ; j ++ )
      {
         for ( i = 0 ; i < numOriVerts ; i ++ )
         {
            x[i] = orivertlist[ 3 * i + j ];
         }
         LinearSystemSolver::solveD( numvar, ata, atb[j], x, SOLVE_CONJUGATEGRADIENT ) ;
         float avg = 0 ;
         for ( i = 0 ; i < numOriVerts ; i ++ )
         {
            orivertlist[ 3 * i + j ] = x[i] ;
            //                                if ( i != centind )
            {
               avg += x[i] ;
            }
         }
         //                        orivertlist[ 3 * centind + j ] = avg / ( numOriVerts - 1 ) ;
      }

      DebugHelper::CheckMemory();

      // De-allocate ata, atb
      /** DEH
      delete[] maskl;
      delete[] atb ;
      delete[] x ;
      for ( i = 0 ; i < numvar ; i ++ )
      {
         delete[] ata[i] ;
      }
      delete[] ata ;
      for ( i = 0 ; i < numrows ; i ++ )
      {
         delete[] a[i] ;
      }
      */
/** DEH      for ( i = 0 ; i < 3 ; i ++ )
      {
         delete[] b[i] ;
      } */
      /// DEH: delete[] a ;
      /// DEH: delete[] b ;

      printf("Fitting done.\n") ;

   }

  };
}

void Registration::AlignLS(PolyMesh&   aMesh,
                           VertexList& aLandmarks)
{
   PolyMesh subMesh;
   subMesh.clone(aMesh);
   SubdivisionAlgorithm::SubdivideMesh(subMesh, cSUB_LEVEL);

   alignCode code;
   int c;
   std::vector<float> verts;
   for (size_t i = 0; i < aMesh.verts.size(); ++i)
   {
      Vec3& v = aMesh.verts[i];
      verts.insert(verts.end(), &v[0], &v[0]+3);
   }
   int nCreaseVerts=0;
   int* creaseVerts=0;
   int nCreaseEdges = aMesh.crease_edges.size();
   int* creaseEdges = new int[nCreaseEdges*2];
   for (size_t i = 0; i < aMesh.crease_edges.size(); ++i)
   {
      creaseEdges[i*2] = aMesh.crease_edges[i].first;
      creaseEdges[i*2+1] = aMesh.crease_edges[i].second;
   }
   int nTris = aMesh.crease_faces.size();
   std::vector<int> faceMask(nTris*2);
   int* tris = new int[nTris*3];
   for (size_t i = 0; i < aMesh.crease_faces.size(); ++i)
   {
      Face& f = aMesh.crease_faces[i];
      tris[i*3] = f.vertex[0];
      tris[i*3+1] = f.vertex[1];
      tris[i*3+2] = f.vertex[2];
      faceMask[i*2] = f.materials[0];
      faceMask[i*2+1] = f.materials[1];
   }
   
   int* faceMaskPtr = 0;
   if (nTris > 0) faceMaskPtr = &faceMask[0];
   
   //const int toTheirOctVert[] = { 0, 2, 1, 4, 5, 3};
   const int toTheirOctVert[] = { 0, 2, 1, 4, 5, 3 };
   std::vector<int> tets, octs, tetMask, octMask;
   for (size_t i = 0; i < aMesh.polys.size(); ++i)
   {
      Polyhedron& p = aMesh.polys[i];
      int arity = p.arity;
      std::vector<int>* polys=0;
      if (arity==4)
      {
         polys = &tets;
         tetMask.push_back(p.material);
         for (int j = 0; j < arity; ++j) 
         { 
            polys->push_back(p.vertex[j]);
         }
      }
      if (arity==6)
      {
         polys = &octs;
         octMask.push_back(p.material);
         for (int j = 0; j < arity; ++j) 
         { 
            polys->push_back(p.vertex[toTheirOctVert[j]]);
         }
      }
   }
   
   int nVerts(verts.size() / 3);
   float* vertPtr = &verts[0];
   int nCrVerts(0);
   int* crVerts=0;

   int nTets(tets.size()/4);
   int* tetPtr = 0;
   int* tetMaskPtr = 0;
   if (nTets > 0) 
   {
      tetPtr = &tets[0];
      tetMaskPtr = &tetMask[0];
   }

   int nOcts(octs.size()/6);
   int* octPtr = 0;
   int* octMaskPtr = 0;
   if (nOcts > 0)
   {
      octPtr = &octs[0];
      octMaskPtr = &octMask[0];
   }

   //nTets=0;
   code.vertlist = new float[subMesh.verts.size() * 3];
   for (size_t i = 0; i < subMesh.verts.size(); ++i)
   {
      Vec3& v = subMesh.verts[i];
      code.vertlist[i*3] = v.x;
      code.vertlist[i*3+1] = v.y;
      code.vertlist[i*3+2] = v.z;
   }
   code.numVerts = subMesh.verts.size();
   code.credgelist = creaseEdges;
   code.crvertlist = crVerts;
   code.numCrVerts = nCrVerts;
   code.numCrEdges = nCreaseEdges;
   code.numCrTris = nTris;
   code.crtrilist = tris;
   code.numTets = nTets;
   code.tetlist = tetPtr;
   code.numOcts = nOcts;
   code.octlist = octPtr;
   code.facemasks = faceMaskPtr;
   code.octmasks = octMaskPtr;
   code.tetmasks = tetMaskPtr;
   
   code.numOriVerts = nVerts;
   code.orivertlist = new float[nVerts * 3];
   std::copy(vertPtr, vertPtr + nVerts * 3, code.orivertlist);
   code.sublevel = cSUB_LEVEL;

   code.numLMVerts = aLandmarks.size();
   code.lmvertlist = new float[aLandmarks.size()*3];
   for (size_t i = 0; i < aLandmarks.size(); ++i)
   {
      Vec3& v = aLandmarks[i];
      code.lmvertlist[i*3] = v.x;
      code.lmvertlist[i*3+1] = v.y;
      code.lmvertlist[i*3+2] = v.z;
   }
   code.alignLS(40.0f, 1.0f, 100.0f);
   DebugHelper::CheckMemory();
   //aMesh.verts.resize(nVerts);
   //aMesh.polys.resize(nTets + nOcts);
   //aMesh.crease_edges.resize(nCreaseEdges);
   //aMesh.crease_faces.resize(nTris);
   float* vp = code.orivertlist;
   for (int i = 0; i < nVerts; ++i)
   {
      aMesh.verts[i].set(vp[i*3], vp[i*3+1], vp[i*3+2]);
   }
   /*
   DebugHelper::CheckMemory();
   for (int i = 0; i < nTris; ++i)
   {
      Face& f = aMesh.crease_faces[i];
      f.vertex[0] = tris[i*3];
      f.vertex[1] = tris[i*3+1];
      f.vertex[2] = tris[i*3+2];
      f.materials[0] = faceMaskPtr[i*2];
      f.materials[1] = faceMaskPtr[i*2+1];
   }
   DebugHelper::CheckMemory();
   for (int i = 0; i < nCreaseEdges; ++i)
   {
      aMesh.crease_edges[i].first = creaseEdges[i*2];
      aMesh.crease_edges[i].second = creaseEdges[i*2+1];
   }
   DebugHelper::CheckMemory();
   //const int tettris[4][3]={{0,1,2},{0,2,3},{0,3,1},{1,3,2}};
   //static const int sTetFaces[4*3];// = { 0,1,2, 3,1,0, 3,2,1, 3,0,2 };
   for (int i = 0; i < nTets; ++i)
   {
      for (int j = 0; j < 4; ++j)
         aMesh.polys[i].vertex[j] = tetPtr[i*4+j];
      aMesh.polys[i].material = tetMaskPtr[i];
      aMesh.polys[i].arity = 4;
   }
   DebugHelper::CheckMemory();
   //const int octtris[8][3]={{0,1,2},{3,4,5},{0,4,3},{0,2,4},{2,5,4},{2,1,5},{1,3,5},{1,0,3}}; // Theirs
   //static const int sOctFaces[8*3];// = { 0,2,1, 0,1,5, 0,5,4, 0,4,2, 3,1,2, 3,5,1, 3,4,5, 3,2,4 }; // Mine
   const int toMyOctVert[] = { 0, 2, 1, 4, 5, 3};
   for (int i = 0; i < nOcts; ++i)
   {
      int octInd(i+nTets);
      for (int j = 0; j < 6; ++j)
         aMesh.polys[octInd].vertex[toMyOctVert[j]] = octPtr[i*6+j];
      aMesh.polys[octInd].material = octMaskPtr[i];
      aMesh.polys[octInd].arity = 6;
   }
   DebugHelper::CheckMemory();
   // UNCOMMENT:::
   //delete[] vertPtr;
   //delete[] crVerts;
   //delete[] tetPtr;
   //delete[] octPtr;
   //delete[] tetMaskPtr;
   //delete[] octMaskPtr;
   //delete[] faceMaskPtr;

   DEBUG_LOG(".\n")

   */
   //aMesh.
   //code.numVerts
}

template <typename Matrix, typename Vector, typename VEC_IN>
bool cg_solve(const Matrix &A, const VEC_IN &b, Vector &x, double tol)
{
   const int N=5;
   using namespace ublas;
   int niter;
   bool conv = false;
   double alpha, beta, residn;

   Vector resid = b - prod(A,x); // residual
   Vector d = resid;             // search direction
   Vector resid_old;
   Vector temp;

   // CG loop
   for(niter = 1; niter <= N; niter++)
   {
      temp = prod(A,d);
      alpha = inner_prod(resid,resid)/inner_prod(d,temp);
      x += (d*alpha);
      resid_old = resid;
      resid -= (temp*alpha);
      residn = norm_2(resid);
      if(residn < tol){
         conv = true;
         break;
      }
      beta = inner_prod(resid,resid)/inner_prod(resid_old,resid_old);
      d = resid + d*beta;
   }

   return conv;
}

static float getVolume(Vec3& a, Vec3& b, Vec3& c, Vec3& d)
{
   Vec3 na = b - a;
   Vec3 nb = c - b;
   Vec3 nc = c - d;
   Vec3 n = cross(a,b);
   return c.dot(n)/6.0f;
}

void Registration::AlignLS2(PolyMesh&     aMesh,
                            MatPointList& aLandmarks,
                            float         fitWt,
                            float         energyWt,
                            int           subLevel)
{
   PolyMesh subMesh;
   subMesh.clone(aMesh);
   SubdivisionAlgorithm::SubdivideMesh(subMesh, subLevel);

   //int i,j,k ;
   int maxIterations = 100 ;
   int fitSubLevel = 3 ;
   int energySubLevel = 2 ;

   FaceList& faces = *aMesh.crease_faces;
   VertexList& verts = *aMesh.verts;
   
   // Find interior vertices
   int* vertflag = new int[ verts.size() ] ;
   for (int i = 0 ; i < verts.size() ; i ++ )
   {
      vertflag[ i ] = 0 ;
   }
   for (int i = 0 ; i < faces.size() ; i ++ )
   {
      for (int j = 0 ; j < 3 ; j ++ )
      {
         vertflag[ faces[i].vertex[j] ] = 1 ;
      }
   }

   // allocate ata, atb, x and temporary vars
   printf("Allocating...\n") ;
   int numvar = verts.size();
   typedef boost::multi_array<float, 2> array_float2;
   //array_float2 ata(boost::extents[numvar][numvar]);
   //array_float2 atb(boost::extents[3][numvar]);
   
   //std::fill(atb.data(), atb.data() + atb.size(), 0.0f);
   //std::fill(ata.data(), ata.data() + atb.size(), 0.0f);

   // Subdivide mask
   printf("Subdividing...\n") ;
   float* maskl = new float[verts.size() * verts.size()];
   for (size_t i = 0; i < verts.size(); ++i)
   {
      int ind = verts.size() * i;
      for (size_t j = 0; j < verts.size(); ++j)
      {
         maskl[ind + j] = (j==i) ? 1 : 0;
      }
   }

   // Subdivide the mask
   PolyMesh subMaskMesh;
   subMaskMesh.clone(aMesh);
   maskl = SubdivisionAlgorithm::SubdivideMask(subMaskMesh, subLevel, maskl, verts.size());
   
   //int numIntFaces = (4 * aMesh.polys.size() - aMesh.crease_faces.size()) / 2;
   std::vector<MeshAlgorithm::TetPair> tetPairs;
   MeshAlgorithm::GetTetPairs(aMesh, tetPairs);
   int numIntFaces = tetPairs.size();
   int numrows = aLandmarks.size() + numIntFaces;

   printf("Adding fit terms...\n") ;
   //float** a = new float * [ numrows ] ;
   typedef boost::numeric::ublas::mapped_matrix<float> SparseMatrix;
   typedef ublas::matrix<float> Matrix;
   SparseMatrix a(numrows, numvar);
   //array_float2 a(boost::extents[numrows][numvar]);
   //std::fill(a.data(), a.data() + a.size(), 0.0f);
   //array_float2 b(boost::extents[3][numrows]);
   //std::fill(b.data(), b.data() + b.size(), 0.0f);
   Matrix b(3, numrows);

   // collect list of vertices used in crease faces
   std::vector<int> creaseFaceVertices;
   for (size_t i = 0; i < subMesh.crease_faces.size(); ++i)
   {
      Face& f = subMesh.crease_faces[i];
      creaseFaceVertices.insert(creaseFaceVertices.end(), f.vertex.begin(), f.vertex.end());
   }
   std::sort(creaseFaceVertices.begin(), creaseFaceVertices.end());
   creaseFaceVertices.erase(std::unique(creaseFaceVertices.begin(), creaseFaceVertices.end()), creaseFaceVertices.end());

   MatPointList subMeshMatPoints;
   MeshAlgorithm::ExtractMatPoints(subMesh.GetCreaseMesh(), subMeshMatPoints);
   for (int i = 0 ; i < aLandmarks.size(); ++i)
   {
      // for each landmark, find closest mesh vertex
      MatPoint& landmark = aLandmarks[i];
      Vec3 lm = aLandmarks[i].point;
      int ind = -1 ;
      float mindis2 = 1000000.0f * 1000000.0f;
      //for (int j = 0 ; j < creaseFaceVertices.size(); ++j)
      for (size_t j = 0; j < subMeshMatPoints.size(); ++j)
      {
         // TODO: Only consider surface vertices [ dont need check? ]
         MatPoint& pt = subMeshMatPoints[j];
         if (pt.materials == landmark.materials)
         {
            int fi = j;
            float dis = (subMesh.verts[fi] - lm).length_squared();
            if ( dis < mindis2 )
            {
               mindis2 = dis ;
               ind = fi ;
            }
         }
      }

      if (ind >= 0)
      {
         // Get A
         for (int j = 0 ; j < numvar ; j ++ )
         {
            float val = maskl[verts.size() * ind + j] * fitWt;
            a(i,j) = val;
         }

         // Get b for each x,y,z component
         for (int j = 0 ; j < 3 ; ++j)
         {
            b(j,i) = lm[ j ] * fitWt ;
         }
      }
   }


   // Add energy terms
   printf("Adding energy terms...\n") ;

   // Find all interior faces
   int tettip[4]={3,1,2,0};
   //std::vector<int> intfaces;
   //std::vector<boost::array<int, 3> > intfaces;

   int ind = aLandmarks.size() ;
   for (int i = 0 ; i < tetPairs.size() ; i ++ )
   {
      MeshAlgorithm::TetPair& p = tetPairs[i];
      
      // Get relevant vertices
      int ip1 = p.tetTips[0];
      int ip2 = p.tetTips[1];
      int im1 = p.verts[0];
      int im2 = p.verts[1];
      int im3  = p.verts[2];
      Vec3 p1 = verts[ip1];
      Vec3 p2 = verts[ip2];
      Vec3 m1 = verts[im1];
      Vec3 m2 = verts[im2];
      Vec3 m3 = verts[im3];

      // Get different volumes
      float vol1 = getVolume( m3,m2,m1,p2 ) ;
      float vol2 = getVolume( m1,m2,m3,p1 ) ;
      float vol3 = getVolume( p1,m3,m2,p2 ) ;
      float vol4 = getVolume( p1,m1,m3,p2 ) ;
      float vol5 = getVolume( p1,m2,m1,p2 ) ;
      float normvol = vol1 + vol2 ;
      normvol *= normvol ;
      if (normvol > 1.0E-12)
      {
         a(ind,ip1) = vol1 / normvol * energyWt ;
         a(ind,ip2) = vol2 / normvol * energyWt ;
         a(ind,im1) = -vol3 / normvol * energyWt ;
         a(ind,im2) = -vol4 / normvol * energyWt ;
         a(ind,im3) = -vol5 / normvol * energyWt ;
      }

      //if (!(math::is_finite(a[ind][ip1]) && 
      //math::is_finite(a[ind][ip2])&& 
      //math::is_finite(a[ind][im1])&& 
      //math::is_finite(a[ind][im2])&& 
      //math::is_finite(a[ind][im3])))
      //{
      //   int stophere=1;
      //}
      for (int j = 0 ; j < 3 ; j ++ )
      {
         b(j,ind) = 0;
      }
      ind ++ ;
   }


   printf("Adding to matrix...\n") ;
   DebugHelper::CheckMemory();
   
   Matrix ata = ublas::prod(a, ublas::trans(a));
   Matrix atb = ublas::prod(a, ublas::trans(b));
   //ata = a * aT;

   // create pointer arrays to pass to LinearSystemSolver 
   //float** aRows = new float*[a.size1()];
   //for (size_t i = 0; i < a.size1(); ++i)
   //{
   //   aRows[i] = &a(i,0); 
   //}

   //float** bRows = new float*[b.shape()[0]];
   //for (size_t i = 0; i < b.shape()[0]; ++i)
   //{
   //   bRows[i] = &b[i][0]; 
   //}

   //float** ataRows = new float*[ata.size1()];
   //for (size_t i = 0; i < ata.size1(); ++i)
   //{
   //   ataRows[i] = &ata(i,0); 
   //}

   //float** atbRows = new float*[atb.shape()[0]];
   //for (size_t i = 0; i < atb.shape()[0]; ++i)
   //{
   //   atbRows[i] = &atb[i][0]; 
   //}

   //array_float2 ata(boost::extents[numvar][numvar]);

   
   //LinearSystemSolver::matrixMultiply( numrows, numvar, aRows, numrows, numvar, aRows, ataRows,1, 0 ) ;
   
   //for (int j = 0 ; j < 3 ; j ++ )
   //{
   //   ublas::matrix_row<Matrix> bRow(b, j);
   //   Matrix atb1 = ublas::prod(a, bRow);
   //   LinearSystemSolver::matrixMultiply( numrows, numvar, aRows, numrows, bRows[j], atbRows[j], 1) ;
   //}


   // Solving
   printf("Solving...\n") ;
   //std::vector<float> x(numrows);
   ublas::vector<float> x(numrows);
   for (int j = 0 ; j < 3 ; j ++ )
   {
      for (int i = 0 ; i < verts.size() ; i ++ )
      {
         x[i] = verts[i][j];
      }
      ublas::vector<float> atbRow = ublas::matrix_column<Matrix>(atb, j);
      cg_solve(ata, atbRow, x, .01);
      LinearSystemSolver::solveD( numvar, ataRows, atbRows[j], &x[0], SOLVE_CONJUGATEGRADIENT ) ;
      //LinearSystemSolver::solveD( numvar, &ata(0,0), &atb(j,0), &x[0], SOLVE_CONJUGATEGRADIENT ) ;
      
      float avg = 0 ;
      for (int i = 0 ; i < verts.size() ; i ++ )
      {
         verts[i][j] = x[i];
         avg += x[i] ;
      }
   }

   //delete[] aRows;
   //delete[] bRows;
   //delete[] ataRows;
   //delete[] atbRows;

}
